#ifndef __ROB_COMMUNICATION_H__
#define __ROB_COMMUNICATION_H__

#include "rob_common.h"
#include "crc.h"  /* Include EASML CRC library */

/* Communication Protocol Configuration */
#define ROB_COMM_HEADER_SIZE 4u
#define ROB_COMM_MAX_DATA_SIZE 64u
#define ROB_COMM_MAX_PACKET_SIZE (ROB_COMM_HEADER_SIZE + ROB_COMM_MAX_DATA_SIZE + 2u)
#define ROB_COMM_SYNC_BYTE1 0xAAu
#define ROB_COMM_SYNC_BYTE2 0x55u

/* Command Types */
typedef enum {
    ROB_CMD_SERVO_MOVE = 0x01u,
    ROB_CMD_SERVO_MOVE_ALL = 0x02u,
    ROB_CMD_SERVO_CALIBRATE = 0x03u,
    ROB_CMD_KINEMATICS_MOVE = 0x10u,
    ROB_CMD_KINEMATICS_GET_POS = 0x11u,
    ROB_CMD_SENSOR_READ = 0x20u,
    ROB_CMD_SENSOR_CONFIG = 0x21u,
    ROB_CMD_SYSTEM_STATUS = 0x30u,
    ROB_CMD_SYSTEM_RESET = 0x31u,
    ROB_CMD_TRAJECTORY_START = 0x40u,
    ROB_CMD_TRAJECTORY_STOP = 0x41u,
    ROB_CMD_TRAJECTORY_ADD_POINT = 0x42u,
    ROB_CMD_RESPONSE_OK = 0x80u,
    ROB_CMD_RESPONSE_ERROR = 0x81u
} Rob_CmdType_Type;

/* Communication Status */
typedef enum {
    ROB_COMM_STATUS_OK = 0u,
    ROB_COMM_STATUS_ERROR = 1u,
    ROB_COMM_STATUS_CRC_ERROR = 2u,
    ROB_COMM_STATUS_TIMEOUT = 3u,
    ROB_COMM_STATUS_BUFFER_FULL = 4u,
    ROB_COMM_STATUS_INVALID_CMD = 5u,
    ROB_COMM_STATUS_PARSE_ERROR = 6u
} Rob_CommStatus_Type;

/* Packet Header Structure */
typedef struct {
    uint8 SyncByte1_u8;    /* Sync byte 1 (0xAA) */
    uint8 SyncByte2_u8;    /* Sync byte 2 (0x55) */
    uint8 Command_u8;      /* Command type */
    uint8 DataLength_u8;   /* Data length */
} Rob_PacketHeader_Type;

/* Communication Packet Structure */
typedef struct {
    Rob_PacketHeader_Type Header;
    uint8 Data_au8[ROB_COMM_MAX_DATA_SIZE];
    uint16 Crc_u16;        /* CRC16 checksum (using EASML CRC) */
    uint8 PacketLength_u8; /* Total packet length */
} Rob_CommPacket_Type;

/* Servo Move Command Data */
typedef struct {
    uint8 ServoIndex_u8;   /* Servo index (0-5) */
    sint16 Angle_s16;      /* Target angle (degrees * 10) */
    uint16 Time_u16;       /* Movement time (ms) */
} Rob_ServoMoveCmd_Type;

/* Servo Move All Command Data */
typedef struct {
    sint16 Angles_as16[ROB_SERVO_NUM];  /* Target angles (degrees * 10) */
    uint16 Time_u16;                    /* Movement time (ms) */
} Rob_ServoMoveAllCmd_Type;

/* Kinematics Move Command Data */
typedef struct {
    sint16 X_s16;          /* X position (mm * 10) */
    sint16 Y_s16;          /* Y position (mm * 10) */
    sint16 Z_s16;          /* Z position (mm * 10) */
    sint16 Alpha_s16;      /* Pitch angle (degrees * 10) */
    sint16 Beta_s16;       /* Roll angle (degrees * 10) */
    uint16 Time_u16;       /* Movement time (ms) */
} Rob_KinematicsMoveCmd_Type;

/* Sensor Read Response Data */
typedef struct {
    uint8 SensorType_u8;   /* Sensor type */
    sint32 Value_s32;      /* Sensor value (scaled) */
    uint8 Status_u8;       /* Sensor status */
} Rob_SensorReadResp_Type;

/* System Status Response Data */
typedef struct {
    uint8 SystemState_u8;              /* System state */
    uint8 ServoStatus_au8[ROB_SERVO_NUM]; /* Servo status array */
    uint8 SensorStatus_au8[ROB_SENSOR_NUM]; /* Sensor status array */
    sint16 CurrentAngles_as16[ROB_SERVO_NUM]; /* Current angles (degrees * 10) */
    uint16 BatteryVoltage_u16;         /* Battery voltage (mV) */
    uint16 Temperature_u16;            /* System temperature (0.1°C) */
} Rob_SystemStatusResp_Type;

/* Trajectory Point Data */
typedef struct {
    uint8 ServoIndex_u8;   /* Servo index */
    sint16 Angle_s16;      /* Target angle (degrees * 10) */
    uint16 Time_u16;       /* Time to reach (ms) */
    uint8 InterpolationType_u8; /* Interpolation type */
} Rob_TrajectoryPointCmd_Type;

/* Receive Buffer Structure */
typedef struct {
    uint8 Buffer_au8[ROB_COMM_MAX_PACKET_SIZE * 2]; /* Double buffer */
    uint16 WriteIndex_u16;    /* Write pointer */
    uint16 ReadIndex_u16;     /* Read pointer */
    uint16 BytesAvailable_u16; /* Available bytes */
    boolean PacketReady_b;    /* Packet ready flag */
} Rob_CommBuffer_Type;

/* Communication State Machine */
typedef enum {
    ROB_COMM_STATE_WAIT_SYNC1 = 0u,
    ROB_COMM_STATE_WAIT_SYNC2 = 1u,
    ROB_COMM_STATE_WAIT_CMD = 2u,
    ROB_COMM_STATE_WAIT_LENGTH = 3u,
    ROB_COMM_STATE_WAIT_DATA = 4u,
    ROB_COMM_STATE_WAIT_CRC = 5u,
    ROB_COMM_STATE_PROCESS = 6u
} Rob_CommState_Type;

/* Communication Context Structure */
typedef struct {
    Rob_CommBuffer_Type RxBuffer;
    Rob_CommBuffer_Type TxBuffer;
    Rob_CommPacket_Type CurrentPacket;
    Rob_CommState_Type ParseState;
    uint16 ExpectedBytes_u16;
    uint16 ReceivedBytes_u16;
    uint32 LastRxTime_u32;
    uint32 RxTimeout_u32;
    Rob_CommStatus_Type Status;
} Rob_CommContext_Type;

/* Statistics Structure */
typedef struct {
    uint32 PacketsReceived_u32;
    uint32 PacketsSent_u32;
    uint32 CrcErrors_u32;
    uint32 TimeoutErrors_u32;
    uint32 ParseErrors_u32;
} Rob_CommStats_Type;

/* Function Declarations */

/* Initialization Functions */
Rob_CommStatus_Type Rob_CommInit(Rob_CommContext_Type *Context_pst,
                                uint32 RxTimeout_u32);

/* Packet Building Functions */
Rob_CommStatus_Type Rob_CommBuildPacket(Rob_CommPacket_Type *Packet_pst,
                                       Rob_CmdType_Type Command_e,
                                       const uint8 *Data_cpu8,
                                       uint8 DataLength_u8);

Rob_CommStatus_Type Rob_CommAddCrc(Rob_CommPacket_Type *Packet_pst);

/* Packet Parsing Functions */
Rob_CommStatus_Type Rob_CommReceiveByte(Rob_CommContext_Type *Context_pst,
                                       uint8 ReceivedByte_u8,
                                       uint32 CurrentTime_u32);

Rob_CommStatus_Type Rob_CommParsePacket(Rob_CommContext_Type *Context_pst,
                                       Rob_CommPacket_Type *Packet_pst);

Rob_CommStatus_Type Rob_CommValidatePacket(const Rob_CommPacket_Type *Packet_cpst);

/* Command Processing Functions */
Rob_CommStatus_Type Rob_CommProcessCommand(const Rob_CommPacket_Type *Packet_cpst,
                                          Rob_CommPacket_Type *Response_pst);

Rob_CommStatus_Type Rob_CommProcessServoMove(const Rob_ServoMoveCmd_Type *Cmd_cpst);

Rob_CommStatus_Type Rob_CommProcessServoMoveAll(const Rob_ServoMoveAllCmd_Type *Cmd_cpst);

Rob_CommStatus_Type Rob_CommProcessKinematicsMove(const Rob_KinematicsMoveCmd_Type *Cmd_cpst);

/* Response Building Functions */
Rob_CommStatus_Type Rob_CommBuildResponse(Rob_CommPacket_Type *Response_pst,
                                         Rob_CmdType_Type ResponseType_e,
                                         const void *ResponseData_cpv,
                                         uint8 DataLength_u8);

Rob_CommStatus_Type Rob_CommBuildErrorResponse(Rob_CommPacket_Type *Response_pst,
                                              Rob_CommStatus_Type ErrorCode_e);

/* Transmission Functions */
Rob_CommStatus_Type Rob_CommSendPacket(Rob_CommContext_Type *Context_pst,
                                      const Rob_CommPacket_Type *Packet_cpst);

Rob_CommStatus_Type Rob_CommGetTxData(Rob_CommContext_Type *Context_pst,
                                     uint8 *Data_pu8, uint16 *Length_pu16);

/* Utility Functions (using EASML CRC) */
uint16 Rob_CommCalculateCrc16_u16(const uint8 *Data_cpu8, uint16 Length_u16);

boolean Rob_CommVerifyCrc_b(const Rob_CommPacket_Type *Packet_cpst);

/* Data Conversion Functions */
sint16 Rob_CommFloatToFixed_s16(float32 Value_f32, uint8 FracBits_u8);

float32 Rob_CommFixedToFloat_f32(sint16 Value_s16, uint8 FracBits_u8);

/* Buffer Management */
Rob_CommStatus_Type Rob_CommBufferWrite(Rob_CommBuffer_Type *Buffer_pst,
                                       const uint8 *Data_cpu8, uint16 Length_u16);

Rob_CommStatus_Type Rob_CommBufferRead(Rob_CommBuffer_Type *Buffer_pst,
                                      uint8 *Data_pu8, uint16 Length_u16);

uint16 Rob_CommBufferAvailable_u16(const Rob_CommBuffer_Type *Buffer_cpst);

void Rob_CommBufferReset(Rob_CommBuffer_Type *Buffer_pst);

/* Statistics Functions */
void Rob_CommGetStats(const Rob_CommContext_Type *Context_cpst,
                     Rob_CommStats_Type *Stats_pst);

void Rob_CommResetStats(Rob_CommContext_Type *Context_pst);

#endif /* __ROB_COMMUNICATION_H__ */
